#pragma once

#include "Math.h"

template <typename T> 
class BasicPoint 
{
public:
	T x;
	T y;

	BasicPoint(T _x = 0, T _y = 0): x(_x), y(_y) 
	{
	}

	BasicPoint(const BasicPoint<T>& rhs): x(rhs.x), y(rhs.y) 
	{
	}

	BasicPoint<T> operator+(const BasicPoint<T>& p) const 
	{
		return BasicPoint<T>(x + p.x, y + p.y);
	}

	BasicPoint<T> operator-(const BasicPoint<T>& p) const 
	{
		return BasicPoint<T>(x - p.x, y - p.y);
	}

	BasicPoint<T>& operator+=(const BasicPoint<T>& p) 
	{
		x += p.x;
		y += p.y;
		return *this;
	}

	BasicPoint<T>& operator-=(const BasicPoint<T>& p) 
	{
		x -= p.x;
		y -= p.y;
		return *this;
	}

	BasicPoint<T> operator*(const T& i) const 
	{
		return BasicPoint<T>(x * i, y * i);
	}

	BasicPoint<T> operator/(const T& i) const 
	{
		return BasicPoint<T>(x / i, y / i);
	}

	bool operator==(const BasicPoint<T>& p) const 
	{
		return x == p.x && y == p.y;
	}

	bool operator!=(const BasicPoint<T>& p) const 
	{
		return !(x == p.x && y == p.y);
	}

	T length() const 
	{
		double sq;
		sq = x*x + y*y;
		return static_cast<T>(Mathd::Sqrt(sq));
	}

	void normalize() 
	{
		T invLength = 1.0/length();

		//TODO: get rid of this static cast
		if (invLength > static_cast<T>(Mathd::zeroTolerance())) {
			x = x * invLength;
			y = y * invLength;
		}
		else {
			x = 0;
			y = 0;
		}
	}

	void rotate(T angle){
		//TODO: get rid of this static cast
		T theta = (angle * static_cast<T>(Mathd::pi()))/180;
		T costheta = static_cast<T>(Mathd::Cos(theta));
		T sintheta = static_cast<T>(Mathd::Sin(theta));

		T nx = x;
		T ny = y;

		x = costheta * nx - sintheta * ny;
		y = sintheta * nx + costheta * ny;
	}

	void rotate(const BasicPoint<T>& origin, T angle){
		//TODO: get rid of this static cast
		T theta = (angle * static_cast<T>(Mathd::pi()))/180;
		T costheta = static_cast<T>(Mathd::Cos(theta));
		T sintheta = static_cast<T>(Mathd::Sin(theta));

		T nx = x - origin.x;
		T ny = y - origin.y;

		x = costheta * nx - sintheta * ny;
		y = sintheta * nx + costheta * ny;
	}

};

template<typename T>
std::ostream& operator<<(std::ostream& os, const BasicPoint<T>& p) 
{
	return os << "(" << p.x << ":" << p.y << ")";
}

typedef BasicPoint<int> Point;
typedef BasicPoint<double> DoublePoint;


